// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and
// the qglviewer library : http://www.libqglviewer.com


#include "OpenMeca/Geom/Quaternion.hpp"
#include "OpenMeca/Geom/Matrix.hpp" 

namespace OpenMeca
{
  namespace Geom
  {

    template<>
    Vector<_3D>
    Quaternion<_3D>::GetAxis() const
    {
      
      return ToVector(GetFrameFunctionAccess()).Unit();
    }

    template<>
    double 
    Quaternion<_3D>::GetAngle() const
    {
      //assert (real_ >=-1 && real_ <=1);
      return  2.0*acos(real_);
    }

    template<>
    void
    Quaternion<_3D>::GetAxisAngle(Vector<_3D>& axis, double& angle) const
    {
      angle = GetAngle();
      axis = GetAxis();
    }

    template<>
    Matrix<_3D>  
    Quaternion<_3D>::ToRotationMatrix() const
    {
      boost::function<const Frame<_3D>& ()> f = boost::bind(&Quaternion<_3D>::GetFrameTo, boost::ref(*this));
      Matrix<_3D> m(f);
      m[0][0] = 1. - 2.*q_[1]*q_[1] - 2*q_[2]*q_[2];
      m[1][1] = 1. - 2.*q_[0]*q_[0] - 2*q_[2]*q_[2];
      m[2][2] = 1. - 2.*q_[0]*q_[0] - 2*q_[1]*q_[1];

      m[0][1] = 2*q_[0]*q_[1] - 2*q_[2]*real_;
      m[1][0] = 2*q_[0]*q_[1] + 2*q_[2]*real_;

      m[0][2] = 2*q_[0]*q_[2] + 2*q_[1]*real_;
      m[2][0] = 2*q_[0]*q_[2] - 2*q_[1]*real_;

      m[1][2] = 2*q_[1]*q_[2] - 2*q_[0]*real_;
      m[2][1] = 2*q_[1]*q_[2] + 2*q_[0]*real_;

      return m;
    }


  }
}


